{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "\n",
    "# metrics.py API & Algorithm Documentation\n",
    "\n",
    "This notebook documents the API and the theory behind the core metrics."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "## Setup"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "outputs": [],
   "source": [
    "from evo.core import metrics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "**...some additional modules and settings for this demo:**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "from evo.tools import log\n",
    "log.configure_logging(verbose=True, debug=True, silent=False)\n",
    "\n",
    "import pprint\n",
    "import numpy as np\n",
    "\n",
    "from evo.tools import plot\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "%matplotlib notebook\n",
    "\n",
    "# temporarily override some package settings\n",
    "from evo.tools.settings import SETTINGS\n",
    "SETTINGS.plot_usetex = False"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "**Load two example trajectory files in TUM format;**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false,
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "from evo.tools import file_interface\n",
    "\n",
    "ref_file = \"../test/data/freiburg1_xyz-groundtruth.txt\"\n",
    "est_file = \"../test/data/freiburg1_xyz-rgbdslam_drift.txt\"\n",
    "\n",
    "traj_ref = file_interface.read_tum_trajectory_file(ref_file)\n",
    "traj_est = file_interface.read_tum_trajectory_file(est_file)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    " **The metrics require the trajectories to be associated via matching timestamps:**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from evo.core import sync\n",
    "\n",
    "max_diff = 0.01\n",
    "\n",
    "traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, max_diff)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Optionally, trajectory points can be aligned. Since we know the data associations, the alignment can be calculated in closed form using Umeyama's method [Umeyama-1991]. Additionally / alternatively, the scale can be corrected (e.g. for monocular SLAM).**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import copy\n",
    "\n",
    "traj_est_aligned = copy.deepcopy(traj_est)\n",
    "traj_est_aligned.align(traj_ref, correct_scale=False, correct_only_scale=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Plot the trajectories:**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fig = plt.figure()\n",
    "traj_by_label = {\n",
    "    \"estimate (not aligned)\": traj_est,\n",
    "    \"estimate (aligned)\": traj_est_aligned,\n",
    "    \"reference\": traj_ref\n",
    "}\n",
    "plot.trajectories(fig, traj_by_label, plot.PlotMode.xyz)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "<a id='ape_math'></a>\n",
    "## APE\n",
    "***The absolute pose error is a metric for investigating the global consistency of a SLAM trajectory***\n",
    "\n",
    "APE is based on the absolute relative pose between two poses $P_{ref,i}, P_{est,i} \\in \\mathrm{SE}(3)$ at timestamp $i$:\n",
    "\\begin{equation*}\n",
    "E_i = P_{est,i} \\ominus P_{ref,i} = P_{ref,i}^{-1} P_{est,i} \\in \\mathrm{SE}(3)\n",
    "\\end{equation*}\n",
    "where $\\ominus$ is the inverse compositional operator, which takes two poses and gives the relative pose [Lu-1997].\n",
    "You can use different pose relations to calculate the APE:\n",
    "* **`metrics.PoseRelation.translation_part`**\n",
    "    * this uses the translation part of $E_i$\n",
    "    * $ APE_i = \\| \\mathrm{trans}(E_i) \\| $\n",
    "* **`metrics.PoseRelation.rotation_angle_(rad/deg)`**\n",
    "    * uses the rotation angle of $E_i$\n",
    "    * $ APE_i = |( \\mathrm{angle}(\\log_{\\mathrm{SO}(3)}(\\mathrm{rot}(E_i)) )| $\n",
    "    * $ \\log_{\\mathrm{SO}(3)}(\\cdot) $ is the inverse of $ \\exp_{\\mathfrak{so}(3)}(\\cdot) $ (Rodrigues' formula)\n",
    "* **`metrics.PoseRelation.rotation_part`**\n",
    "    * this uses the rotation part of $E_i$\n",
    "    * $ APE_i = \\| \\mathrm{rot}(E_i) - I_{3 \\times 3} \\|_F $\n",
    "    * unit-less\n",
    "* **`metrics.PoseRelation.full_transformation`**\n",
    "    * this uses the full relative pose $E_i$\n",
    "    * $ APE_i = \\| E_i - I_{4 \\times 4} \\|_F $\n",
    "    * unit-less\n",
    "    \n",
    "Then, different statistics can be calculated on the APEs of all timestamps, e.g. the RMSE:\n",
    "\\begin{equation*}\n",
    "\\mathrm{RMSE} = \\sqrt{ \\frac{1}{N} \\sum_{i=1}^N APE_i^2 } \n",
    "\\end{equation*}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Settings"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "pose_relation = metrics.PoseRelation.translation_part\n",
    "use_aligned_trajectories = False"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Data Preparation\n",
    "Optionally, we can use the aligned trajectory:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "if use_aligned_trajectories:\n",
    "    data = (traj_ref, traj_est_aligned) \n",
    "else:\n",
    "    data = (traj_ref, traj_est)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Run APE on Data\n",
    "Create an instance of the APE class and process the data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "ape_metric = metrics.APE(pose_relation)\n",
    "ape_metric.process_data(data)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true,
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Get APE Statistics\n",
    "Get a single statistic:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "ape_stat = ape_metric.get_statistic(metrics.StatisticsType.rmse)\n",
    "print(ape_stat)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "Get all avalaible statistics at once in a dictionary:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "ape_stats = ape_metric.get_all_statistics()\n",
    "pprint.pprint(ape_stats)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the APE values and statistics:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "seconds_from_start = [t - traj_est.timestamps[0] for t in traj_est.timestamps]\n",
    "fig = plt.figure()\n",
    "plot.error_array(fig.gca(), ape_metric.error, x_array=seconds_from_start,\n",
    "                 statistics={s:v for s,v in ape_stats.items() if s != \"sse\"},\n",
    "                 name=\"APE\", title=\"APE w.r.t. \" + ape_metric.pose_relation.value, xlabel=\"$t$ (s)\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the trajectory with colormapping of the APE:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "plot_mode = plot.PlotMode.xy\n",
    "fig = plt.figure()\n",
    "ax = plot.prepare_axis(fig, plot_mode)\n",
    "plot.traj(ax, plot_mode, traj_ref, '--', \"gray\", \"reference\")\n",
    "plot.traj_colormap(ax, traj_est_aligned if use_aligned_trajectories else traj_est, ape_metric.error, \n",
    "                   plot_mode, min_map=ape_stats[\"min\"], max_map=ape_stats[\"max\"])\n",
    "ax.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Comparison with TUM ATE script"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The official TUM RGB-D benchmark script `evaluate_ate.py` [Sturm-2012] gives the RMSE of the xyz difference of the aligned trajectories:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import subprocess as sp\n",
    "import sys\n",
    "cmd = [\"python\", \"../test/tum_benchmark_tools/evaluate_ate.py\", ref_file, est_file, \"--max_difference\", str(max_diff)]\n",
    "out = sp.check_output(cmd)\n",
    "print(out.decode(sys.stdout.encoding))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "...which is equivalent to:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tum_ate_equivalent = metrics.APE(metrics.PoseRelation.translation_part)\n",
    "tum_ate_equivalent.process_data((traj_ref, traj_est_aligned))\n",
    "print(tum_ate_equivalent.get_statistic(metrics.StatisticsType.rmse))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<a id='rpe_math'></a>\n",
    "# RPE\n",
    "\n",
    "***The relative pose error is a metric for investigating the local consistency of a SLAM trajectory***\n",
    "\n",
    "RPE compares the relative poses along the estimated and the reference trajectory. This is based on the delta pose difference: \n",
    "\\begin{equation*} E_{i,j} = \\delta_{est_{i,j}} \\ominus \\delta_{ref_{i,j}} = (P_{ref,i}^{-1}P_{ref,j})^{-1} (P_{est,i}^{-1}P_{est,j}) \\in \\mathrm{SE}(3) \\end{equation*}\n",
    "\n",
    "You can use different pose relations to calculate the RPE from timestamp $i$ to $j$:\n",
    "* **`metrics.PoseRelation.translation_part`**\n",
    "    * this uses the translation part of $E_{i,j}$\n",
    "    * $ RPE_{i,j} = \\| \\mathrm{trans}(E_{i,j}) \\| $\n",
    "* **`metrics.PoseRelation.rotation_angle_(rad/deg)`**\n",
    "    * uses the absolute angular error of $E_{i,j}$\n",
    "    * $ RPE_{i,j} = |( \\mathrm{angle}(\\log_{\\mathrm{SO}(3)}(\\mathrm{rot}(E_{i,j})) )| $\n",
    "    * $ \\log_{\\mathrm{SO}(3)}(\\cdot) $ is the inverse of $ \\exp_{\\mathfrak{so}(3)}(\\cdot) $ (Rodrigues' formula)\n",
    "* **`metrics.PoseRelation.rotation_part`**\n",
    "    * this uses the rotation part of $E_{i,j}$\n",
    "    * $ RPE_{i,j} = \\| \\mathrm{rot}(E_{i,j}) - I_{3 \\times 3} \\|_F $\n",
    "    * unit-less\n",
    "* **`metrics.PoseRelation.full_transformation`**\n",
    "    * this uses the full delta pose difference $E_{i,j}$\n",
    "    * $ RPE_{i,j} = \\| E_{i,j} - I_{4 \\times 4} \\|_F $\n",
    "    * unit-less\n",
    "    \n",
    "Then, different statistics can be calculated on the RPEs of all timestamps, e.g. the RMSE:\n",
    "\\begin{equation*}\n",
    "\\mathrm{RMSE} = \\sqrt{ \\frac{1}{N} \\sum_{\\forall ~i,j} RPE_{i,j}^2 } \n",
    "\\end{equation*}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Settings\n",
    "\n",
    "The parameter $\\Delta$ determines the distance between the pose pairs along the trajectories. E.g. if you have 30 poses per second and want to measure the RPE every second, use $\\Delta=30 ~\\text{(frames)}$. Or to measure everytime you moved 1 meter, $\\Delta=1 ~\\text{(m)}$, ...\n",
    "\n",
    "Another option is to use all pairs of a certain delta value, i.e. not only the subsequent (linear) delta pairs of the trajectory.\n",
    "\n",
    "Using aligned trajectories does not make sense with RPE because the delta poses are the same as with the unaligned case."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "pose_relation = metrics.PoseRelation.rotation_angle_deg\n",
    "\n",
    "# normal mode\n",
    "delta = 1\n",
    "delta_unit = metrics.Unit.frames\n",
    "\n",
    "# all pairs mode\n",
    "all_pairs = False  # activate"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Data Preparation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "data = (traj_ref, traj_est)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Run RPE on Data\n",
    "Create an instance of the RPE class and process the data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "rpe_metric = metrics.RPE(pose_relation, delta, delta_unit, all_pairs)\n",
    "rpe_metric.process_data(data)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true,
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "### Get RPE Statistics\n",
    "Get a single statistic:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "rpe_stat = rpe_metric.get_statistic(metrics.StatisticsType.rmse)\n",
    "print(rpe_stat)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "Get all avalaible statistics at once in a dictionary:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "outputs": [],
   "source": [
    "rpe_stats = rpe_metric.get_all_statistics()\n",
    "pprint.pprint(rpe_stats)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Plot the RPE values and statistics:**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# important: restrict data to delta ids for plot\n",
    "import copy\n",
    "traj_ref_plot = copy.deepcopy(traj_ref)\n",
    "traj_est_plot = copy.deepcopy(traj_est)\n",
    "traj_ref_plot.reduce_to_ids(rpe_metric.delta_ids)\n",
    "traj_est_plot.reduce_to_ids(rpe_metric.delta_ids)\n",
    "seconds_from_start = [t - traj_est.timestamps[0] for t in traj_est.timestamps[1:]]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fig = plt.figure()\n",
    "plot.error_array(fig.gca(), rpe_metric.error, x_array=seconds_from_start,\n",
    "                 statistics={s:v for s,v in rpe_stats.items() if s != \"sse\"},\n",
    "                 name=\"RPE\", title=\"RPE w.r.t. \" + rpe_metric.pose_relation.value, xlabel=\"$t$ (s)\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plot the trajectory with colormapping of the RPE:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plot_mode = plot.PlotMode.xy\n",
    "fig = plt.figure()\n",
    "ax = plot.prepare_axis(fig, plot_mode)\n",
    "plot.traj(ax, plot_mode, traj_ref_plot, '--', \"gray\", \"reference\")\n",
    "plot.traj_colormap(ax, traj_est_plot, rpe_metric.error, plot_mode, min_map=rpe_stats[\"min\"], max_map=rpe_stats[\"max\"])\n",
    "ax.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Comparison with TUM RPE script"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The official TUM RGB-D benchmark script evaluate_rpe.py uses all pairs of the specified delta (in \"fixed_delta\" mode only!). \n",
    "\n",
    "The default output is the mean of the RPE:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import subprocess as sp\n",
    "delta = 15\n",
    "cmd = cmd = [\"python\", \"../test/tum_benchmark_tools/evaluate_rpe.py\", ref_file, est_file, \"--delta\", str(delta), \"--delta_unit\", 'f', '--fixed_delta']\n",
    "out = sp.check_output(cmd)\n",
    "print(out.decode(sys.stdout.encoding))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "...which is equivalent to:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "tum_rpe_equivalent = metrics.RPE(metrics.PoseRelation.translation_part, delta, metrics.Unit.frames, all_pairs=True)\n",
    "tum_rpe_equivalent.process_data((traj_ref, traj_est))\n",
    "print(tum_rpe_equivalent.get_statistic(metrics.StatisticsType.mean))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## References\n",
    "\n",
    "|   |   |\n",
    "|---|---|\n",
    "| [Kümmerle-2009] | Rainer Kümmerle, Bastian Steder, Christian Dornhege, Michael Ruhnke, Giorgio Grisetti, Cyrill Stachniss, and Alexander Kleiner. On measuring the accuracy of SLAM algorithms. *Autonomous Robots*, 27(4):387–407, 2009.\n",
    "| [Lu-1997] | Feng Lu and Evangelos Milios. Globally consistent range scan alignment for environment mapping. *Autonomous Robots*, 4(4):333–349, 1997.\n",
    "| [Sturm-2012] | Jürgen Sturm, Nikolas Engelhard, Felix Endres, Wolfram Burgard, and Daniel Cremers. A benchmark for the evaluation of RGB-D SLAM systems. *In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems*, pages 573–580. IEEE, 2012.\n",
    "| [Umeyama-1991] | Umeyama, Shinji. Least-squares estimation of transformation parameters between two point patterns. *IEEE Transactions on Pattern Analysis & Machine Intelligence* 4:376-380, 1991."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}